#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

class SubThenPub:
	def __init__(self):
		self.__pub_ = rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10)   #robot_1 is the follower, get its velocity from rostopic odom
		self.__sub_ = rospy.Subscriber('/robot_0/odom', Odometry, self.callback)  #robot_0 is the leader, pub it velocity through rostopic cmd_vel

	def callback(self, data):
		vel_msg = Twist()                               # 'vel_msg' has msg type Twist

		vel_msg.linear.x = data.twist.twist.linear.x    # 'data' has msg type Odometry

		self.__pub_.publish(vel_msg)

		rospy.loginfo("pub velocity:[%0.2f m/s]",vel_msg.linear.x)

def main():
	rospy.init_node('follow_r0', anonymous=True)
	SAPObject = SubThenPub()
	rospy.spin()

if __name__ == "__main__":
	main()



